#include "os/os.h"
#include "os/types.h"
#include "bsp/bsp_drivers.h"

static u64 os_mseconds = 0;
volatile u32 _g_ticks = 0;
static u32 os_timer_task_handler(void *);

static os_timer_t os_timer_head = {
	.prev = &os_timer_head,
	.next = &os_timer_head
};

static os_task_t os_task_head = {
	.next = &os_task_head,
	.handler = os_timer_task_handler
};


void SysTick_Handler(void)
{

#ifdef STM32F405xx
	HAL_IncTick();
#endif
	os_mseconds++;
	_g_ticks ++;
}


u64 os_get_mseconds(void)
{
	return os_mseconds;
}

u32 os_get_seconds(void){
	return _g_ticks/1000;
}

u32 os_timestemp(void) {
	return _g_ticks;
}

u32 os_delta_ms(u32 prev_ms){
	u32 current = os_timestemp();
	if (current >= prev_ms){
		return current - prev_ms;
	}
	return 0xFFFFFFFF - prev_ms + current + 1;
}

void os_delay_ms(u32 ms)
{
	u32 start;
	start = os_timestemp();
	while (os_delta_ms(start) < ms);
}

static inline void os_timer_sync(void)
{
	os_task_head.time = os_timer_head.next->time;
}

void os_timer_post(os_timer_t *timer, u32 delay)
{
	os_timer_t *node;
	u64 time;

	__disable_irq();

	time = os_mseconds + (delay > 0 ? delay : 1);

	if (timer->prev != NULL) {
		timer->prev->next = timer->next;
	}

	if (timer->next != NULL) {
		timer->next->prev = timer->prev;
	}

	for (node = os_timer_head.next; node != &os_timer_head; node = node->next) {
		if (node->time > time) {
			break;
		}
	}

	timer->prev = node->prev;
	node->prev->next = timer;
	node->prev = timer;
	timer->next = node;
	timer->time = time;
	os_timer_sync();

	__enable_irq();
}

void os_timer_cancel(os_timer_t *timer)
{
	__disable_irq();

	timer->prev->next = timer->next;
	timer->next->prev = timer->prev;
	timer->next = timer->prev = timer;
	os_timer_sync();

	__enable_irq();
}

static u32 os_timer_task_handler(void *p)
{
	while (1) {
		os_timer_t *timer = os_timer_head.next;

		if (timer->time > os_mseconds) {
			return timer->time - os_mseconds;
		}

		if (timer != &os_timer_head) {
			os_timer_cancel(timer);
			timer->handler(timer);
		} else {
			break;
		}

		wdog_reload();
	}

	return 0xFFFFFFFF;
}

void os_task_add(os_task_t *task)
{
	__disable_irq();

	task->next = os_task_head.next;
	os_task_head.next = task;

	__enable_irq();
}

os_task_t *os_task_create(TskFunction_t handler, void *params) {
	os_task_t *ntsk = os_alloc(sizeof(os_task_t));
	if (ntsk == NULL) {
		return ntsk;
	}
	ntsk->time = os_mseconds;
	ntsk->handler = handler;
	ntsk->params = params;
	os_task_add(ntsk);
	return ntsk;
}

#ifndef USE_MEX_CMD
void os_task_run(void)
{
	os_task_t *head = &os_task_head;

	while (1) {
		wdog_reload();

		if (head->time <= os_mseconds) {
			head->time = os_mseconds + head->handler(head->params);
		}

		head = head->next;
	}
}
#else
void os_task_run(void)
{
	os_task_t *head = &os_task_head;

	do {
		wdog_reload();

		if (head->time <= os_mseconds) {
			head->time = os_mseconds + head->handler(head->params);
		}

		head = head->next;
	}while(head != &os_task_head);
}
#endif